/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-24 07:09:23
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-13 08:04:44
 * @Description: ...
 * @Reference: ...
 */
#ifndef _VILENS_CONFIG_H_
#define _VILENS_CONFIG_H_

#include "lci_slam/vilens/common_header.h"

namespace vilens {

class config {
    struct lidarConfig {
        std::string lidarTopic;
        bool time_sync_with_imu;
        int scan_line;
        float scan_rate;
        float fov_degree;
        float detection_range;

        // preprocessor values
        bool feature_extract = false;
        int lidar_type = 4;
        float blind = 1;
        int pfilt_num = 1;

        // processor values
        float processor_downsample_resolution_value = 0.25;
        float processor_max_corres_dist_value = 1.0;

        // Adaptive Parameters
        bool adaptive_params = true;
        // Frame
        std::string odom_frame = "odom";
        std::string child_frame = "base_link";

        // Crop Box Filter
        bool preprocessing_cropboxfilter_use = true;
        float preprocessing_cropboxfilter_size = 1.0;
        // Voxel Grid Filter
        bool preprocessing_voxelfilter_scan_use = true;
        float preprocessing_voxelfilter_scan_res = 0.25;
        bool preprocessing_voxelfilter_submap_use = true;
        float preprocessing_voxelfilter_submap_res = 0.5;
        float preprocessing_voxelfilter_map_res = 0.5;

        // Keyframe Threshold
        float keyframe_thresh_D = 5.0;
        float keyframe_thresh_R = 45.0;

        // Submap
        int submap_keyframe_knn = 10;
        int submap_keyframe_kcv = 10;
        int submap_keyframe_kcc = 10;

        // GICP
        int gicp_minnumpoints = 10;
        // Frame2Frame
        int gicp_s2s_kcorrespondences = 10;
        float gicp_s2s_maxcorrespondencedistance = 1.0;
        int gicp_s2s_maxiterations = 32;
        float gicp_s2s_transformationepsilon = 0.01;
        float gicp_s2s_euclideanfitnessepsilon = 0.01;
        int gicp_s2s_ransac_iterations = 5;
        float gicp_s2s_ransac_outlierRejectionThresh = 1.0;
        // Frame2SubMap
        int gicp_s2m_kcorrespondences = 10;
        float gicp_s2m_maxcorrespondencedistance = 1.0;
        int gicp_s2m_maxiterations = 32;
        float gicp_s2m_transformationepsilon = 0.01;
        float gicp_s2m_euclideanfitnessepsilon = 0.01;
        int gicp_s2m_ransac_iterations = 5;
        float gicp_s2m_ransac_outlierRejectionThresh = 1.0;

        Eigen::Matrix4d relPose;

        void load(const YAML::Node &node) {
            if (node) {
                lidarTopic = node["lidar_topic"].as<std::string>();
                time_sync_with_imu = node["time_sync_with_imu"].as<bool>();
                scan_line = node["scan_line"].as<int>();
                scan_rate = node["scan_rate"].as<float>();
                fov_degree = node["fov_degree"].as<float>();
                detection_range = node["detection_range"].as<float>();
                blind = node["blind"].as<float>();

                std::cout << "das0" << std::endl;

                odom_frame = node["odom_frame"].as<std::string>();
                child_frame = node["child_frame"].as<std::string>();

                std::cout << "das1" << std::endl;

                // Keyframe Threshold
                keyframe_thresh_D = node["keyframe"]["threshD"].as<float>();
                keyframe_thresh_R = node["keyframe"]["threshR"].as<float>();

                std::cout << "das2" << std::endl;

                // Submap
                submap_keyframe_knn =
                    node["submap"]["keyframe"]["knn"].as<int>();
                submap_keyframe_kcv =
                    node["submap"]["keyframe"]["kcv"].as<int>();
                submap_keyframe_kcc =
                    node["submap"]["keyframe"]["kcc"].as<int>();

                std::cout << "das2" << std::endl;

                // Crop Box Filter
                preprocessing_cropboxfilter_use =
                    node["preprocessing"]["cropBoxFilter"]["use"].as<bool>();
                preprocessing_cropboxfilter_size =
                    node["preprocessing"]["cropBoxFilter"]["size"].as<float>();

                std::cout << "das3" << std::endl;

                // Voxel Grid Filter
                preprocessing_voxelfilter_scan_use =
                    node["preprocessing"]["voxelFilter"]["scan"]["use"]
                        .as<bool>();
                preprocessing_voxelfilter_scan_res =
                    node["preprocessing"]["voxelFilter"]["scan"]["res"]
                        .as<float>();
                preprocessing_voxelfilter_submap_use =
                    node["preprocessing"]["voxelFilter"]["submap"]["use"]
                        .as<bool>();
                preprocessing_voxelfilter_submap_res =
                    node["preprocessing"]["voxelFilter"]["submap"]["res"]
                        .as<float>();
                preprocessing_voxelfilter_map_res =
                    node["preprocessing"]["voxelFilter"]["map"]["res"]
                        .as<float>();

                std::cout << "das4" << std::endl;

                // Adaptive Parameters
                adaptive_params = node["adaptiveParams"].as<bool>();

                // GICP
                gicp_minnumpoints = node["gicp"]["minNumPoints"].as<int>();
                // Scan2Scan
                gicp_s2s_kcorrespondences =
                    node["gicp"]["s2s"]["kCorrespondences"].as<int>();
                gicp_s2s_maxcorrespondencedistance =
                    node["gicp"]["s2s"]["maxCorrespondenceDistance"]
                        .as<float>();
                gicp_s2s_maxiterations =
                    node["gicp"]["s2s"]["maxIterations"].as<int>();
                gicp_s2s_transformationepsilon =
                    node["gicp"]["s2s"]["transformationEpsilon"].as<float>();
                gicp_s2s_euclideanfitnessepsilon =
                    node["gicp"]["s2s"]["euclideanFitnessEpsilon"].as<float>();
                gicp_s2s_ransac_iterations =
                    node["gicp"]["s2s"]["ransac"]["iterations"].as<int>();
                gicp_s2s_ransac_outlierRejectionThresh =
                    node["gicp"]["s2s"]["ransac"]["outlierRejectionThresh"]
                        .as<float>();
                // Frame2SubMap
                gicp_s2m_kcorrespondences =
                    node["gicp"]["s2m"]["kCorrespondences"].as<int>();
                gicp_s2m_maxcorrespondencedistance =
                    node["gicp"]["s2m"]["maxCorrespondenceDistance"]
                        .as<float>();
                gicp_s2m_maxiterations =
                    node["gicp"]["s2m"]["maxIterations"].as<int>();
                gicp_s2m_transformationepsilon =
                    node["gicp"]["s2m"]["transformationEpsilon"].as<float>();
                gicp_s2m_euclideanfitnessepsilon =
                    node["gicp"]["s2m"]["euclideanFitnessEpsilon"].as<float>();
                gicp_s2m_ransac_iterations =
                    node["gicp"]["s2m"]["ransac"]["iterations"].as<int>();
                gicp_s2m_ransac_outlierRejectionThresh =
                    node["gicp"]["s2m"]["ransac"]["outlierRejectionThresh"]
                        .as<float>();

                relPose = Eigen::Matrix4d::Identity();
                relPose.block<3,3>(0,0) = Eigen::Matrix3d(node["imu"]["extrinsic_R"].as<std::vector<double>>().data());
                relPose.block<3,1>(0,3) = Eigen::Matrix3d(node["imu"]["extrinsic_T"].as<std::vector<double>>().data());

            } else {
                LOG(WARNING) << "Can't load lidar information!!!";
            }
        }

        void print() {
            std::cout << std::endl << "lidar params : " << std::endl;
            std::cout << "  - lidarTopic : " << lidarTopic << std::endl;
            std::cout << "  - time_sync_with_imu : " << (int)time_sync_with_imu
                      << std::endl;
            std::cout << "  - scan_line : " << scan_line << std::endl;
            std::cout << "  - scan_rate : " << scan_rate << std::endl;
            std::cout << "  - fov_degree : " << fov_degree << std::endl;
            std::cout << "  - detection_range : " << detection_range
                      << std::endl;
            std::cout << "  - feature_extract : " << (int)feature_extract
                      << std::endl;
            std::cout << "  - lidar_type : " << lidar_type << std::endl;
            std::cout << "  - blind : " << blind << std::endl;
            std::cout << "  - pfilt_num : " << pfilt_num << std::endl;
            std::cout << "  - processor_downsample_resolution_value : "
                      << processor_downsample_resolution_value << std::endl;
            std::cout << "  - processor_max_corres_dist_value : "
                      << processor_max_corres_dist_value << std::endl;

            std::cout << "  - odom_frame : " << odom_frame << std::endl;
            std::cout << "  - child_frame : " << child_frame << std::endl;
            std::cout << "  - keyframe_thresh_D : " << keyframe_thresh_D
                      << std::endl;
            std::cout << "  - keyframe_thresh_R : " << keyframe_thresh_R
                      << std::endl;
            std::cout << "  - submap_keyframe_knn : " << submap_keyframe_knn
                      << std::endl;
            std::cout << "  - submap_keyframe_kcv : " << submap_keyframe_kcv
                      << std::endl;
            std::cout << "  - submap_keyframe_kcc : " << submap_keyframe_kcc
                      << std::endl;
            std::cout << "  - preprocessing_cropboxfilter_use : "
                      << (int)preprocessing_cropboxfilter_use << std::endl;
            std::cout << "  - preprocessing_cropboxfilter_size : "
                      << preprocessing_cropboxfilter_size << std::endl;
            std::cout << "  - preprocessing_voxelfilter_scan_use : "
                      << (int)preprocessing_voxelfilter_scan_use << std::endl;
            std::cout << "  - preprocessing_voxelfilter_scan_res : "
                      << preprocessing_voxelfilter_scan_res << std::endl;
            std::cout << "  - preprocessing_voxelfilter_submap_use : "
                      << (int)preprocessing_voxelfilter_submap_use << std::endl;
            std::cout << "  - preprocessing_voxelfilter_submap_res : "
                      << preprocessing_voxelfilter_submap_res << std::endl;
            std::cout << "  - preprocessing_voxelfilter_map_res : "
                      << preprocessing_voxelfilter_map_res << std::endl;
            std::cout << "  - adaptive_params : " << (int)adaptive_params
                      << std::endl;
            std::cout << "  - gicp_minnumpoints : " << gicp_minnumpoints
                      << std::endl;

            std::cout << "  - gicp_s2s_kcorrespondences : "
                      << gicp_s2s_kcorrespondences << std::endl;
            std::cout << "  - gicp_s2s_maxcorrespondencedistance : "
                      << gicp_s2s_maxcorrespondencedistance << std::endl;
            std::cout << "  - gicp_s2s_maxiterations : "
                      << gicp_s2s_maxiterations << std::endl;
            std::cout << "  - gicp_s2s_transformationepsilon : "
                      << gicp_s2s_transformationepsilon << std::endl;
            std::cout << "  - gicp_s2s_euclideanfitnessepsilon : "
                      << gicp_s2s_euclideanfitnessepsilon << std::endl;
            std::cout << "  - gicp_s2s_ransac_iterations : "
                      << gicp_s2s_ransac_iterations << std::endl;
            std::cout << "  - gicp_s2s_ransac_outlierRejectionThresh : "
                      << gicp_s2s_ransac_outlierRejectionThresh << std::endl;

            std::cout << "  - gicp_s2m_kcorrespondences : "
                      << gicp_s2m_kcorrespondences << std::endl;
            std::cout << "  - gicp_s2m_maxcorrespondencedistance : "
                      << gicp_s2m_maxcorrespondencedistance << std::endl;
            std::cout << "  - gicp_s2m_maxiterations : "
                      << gicp_s2m_maxiterations << std::endl;
            std::cout << "  - gicp_s2m_transformationepsilon : "
                      << gicp_s2m_transformationepsilon << std::endl;
            std::cout << "  - gicp_s2m_euclideanfitnessepsilon : "
                      << gicp_s2m_euclideanfitnessepsilon << std::endl;
            std::cout << "  - gicp_s2m_ransac_iterations : "
                      << gicp_s2m_ransac_iterations << std::endl;
            std::cout << "  - gicp_s2m_ransac_outlierRejectionThresh : "
                      << gicp_s2m_ransac_outlierRejectionThresh << std::endl;
            std::cout << "  - relPose : "
                      << "                " << relPose << std::endl;

            std::cout << std::endl;
        }
    };

    struct cameraConfig {
        int camNum = 0;
        std::vector<std::string> imageTopics;

        void load(const YAML::Node &node) {
            if (node) {
            } else {
                LOG(WARNING) << "Can't load camera information!!!";
            }
        }
        void print() {
            std::cout << std::endl << "camera params : " << std::endl;
            std::cout << "  - camNum : " << camNum << std::endl;
            std::cout << "  - imageTopics : " << std::endl;
            for (int i = 0; i < camNum; i++) {
                std::cout << "    - topic[" << i << "] : " << imageTopics[i]
                          << std::endl;
            }
            std::cout << std::endl;
        }
    };

    struct imuConfig {
        std::string imuTopic;

        void load(const YAML::Node &node) {
            if (node) {
                imuTopic = node["imu_topic"].as<std::string>();
            } else {
                LOG(WARNING) << "Can't load imu information!!!";
            }
        }
        void print() {
            std::cout << std::endl << "imu params : " << std::endl;
            std::cout << "  - imuTopic : " << imuTopic << std::endl;
            std::cout << std::endl;
        }
    };

public:
    typedef std::shared_ptr<config> Ptr;
    config() {}

    void load(const std::string &config_path) {
        LOG(INFO) << "loading config path -- " << config_path;
        const auto config_node = YAML::LoadFile(config_path);
        if (config_node) {
            LOG(INFO) << "loading imu config ... ";
            imu.load(config_node["imu"]);
            LOG(INFO) << "loading lidar config ... ";
            lidar.load(config_node["lidar"]);
            LOG(INFO) << "loading camera config ... ";
            camera.load(config_node["camera"]);
        } else {
            LOG(ERROR) << "Can't load config file!!!";
            std::exit(EXIT_FAILURE);
        }
    }

    void print() {
        imu.print();
        lidar.print();
        camera.print();
    }

    imuConfig imu;
    lidarConfig lidar;
    cameraConfig camera;
};

}  // namespace vilens

#endif